#include "youbot_relay/move_youbot_base_client.h"


using namespace move_youbot_base;

// ======================================================== //


MoveYoubotBaseClient::MoveYoubotBaseClient(int argc, char** argv)
{
  ROS_INFO("Initializing move_youbot_base client node.");
  ros::init(argc, argv, CLIENT_NODE);
  client = new Client(CLIENT_ACTION, true);
  client->waitForServer( ros::Duration(10,0) );
}

// ======================================================== //

void MoveYoubotBaseClient::setGoal( youbot_relay::MoveYoubotBaseGoal g )
{
  goal = g;
}

// ======================================================== //

void MoveYoubotBaseClient::setGoal(double x_meters, double y_meters,double theta_degree,int velocity_percent)
{
  goal.target_pose.pose.position.x = x_meters;
  goal.target_pose.pose.position.y = y_meters;
  goal.target_pose.pose.orientation = tf::createQuaternionMsgFromYaw(theta_degree*(M_PI/180));//convert deg to rad
  goal.velocity = velocity_percent;
}

// ======================================================== //

bool MoveYoubotBaseClient::sendGoal()
{
  if( /*goal == NULL ||*/ !client->isServerConnected() ) return false;
  client->sendGoal(goal);
  return true;
}

// ======================================================== //

bool MoveYoubotBaseClient::stop()
{
  if( !client->isServerConnected() ) return false;
  client->cancelAllGoals();
  return true;
}

// ======================================================== //

actionlib::SimpleClientGoalState MoveYoubotBaseClient::sendGoalForResult( ros::Duration timeout )
{
  if( /*goal == NULL ||*/ !client->isServerConnected() ) return actionlib::SimpleClientGoalState::ABORTED;
  return client->sendGoalAndWait(goal, timeout);
}

// ======================================================== //

bool MoveYoubotBaseClient::move( double x_meters, double y_meters, double theta_degree, int velocity_percent, ros::Duration timeout)
{
  setGoal(x_meters,y_meters,theta_degree,velocity_percent);

  return move( timeout );
}

// ======================================================== //

bool MoveYoubotBaseClient::move( youbot_relay::MoveYoubotBaseGoal g, ros::Duration timeout )
{
  setGoal(g);

  return move(timeout);
}

// ======================================================== //

bool MoveYoubotBaseClient::moveSynchronous( double x_meters, double y_meters, double theta_degree, int velocity_percent )
{
  setGoal(x_meters,y_meters,theta_degree,velocity_percent);
  if( client->sendGoalAndWait(goal,ros::Duration(0,0)) == actionlib::SimpleClientGoalState::SUCCEEDED ) return true;
  return false;
}

// ======================================================== //

bool MoveYoubotBaseClient::move( ros::Duration timeout )
{
  if( timeout == ros::Duration(0, 0) )//infinite timeout -> do not wait!
  {
    pos_x += goal.target_pose.pose.position.x;//success doesn't matter, add coordinates!
    pos_y += goal.target_pose.pose.position.y;
    pos_theta += tf::getYaw(goal.target_pose.pose.orientation) * (180/M_PI);//Convert rad to deg

    return sendGoal();//send goal
  }
  else//use timeout
  {
    if( sendGoalForResult(timeout) == actionlib::SimpleClientGoalState::SUCCEEDED )
    {
      //!TODO: get odomentry data from server here and fill in below

      pos_x += goal.target_pose.pose.position.x;//add coordinates only on success!
      pos_y += goal.target_pose.pose.position.y;
      pos_theta += tf::getYaw(goal.target_pose.pose.orientation) * (180/M_PI);//Convert rad to deg
      return true;
    }
    else return false;
  }
}

// ======================================================== //

bool MoveYoubotBaseClient::getCurrentPosition( double *x, double *y, double *theta )
{
  if( x==NULL || y == NULL || theta==NULL) return false;//invalid pointers

  *x = pos_x;
  *y = pos_y;
  *theta = pos_theta;

  return true;
}

// ======================================================== //

actionlib::SimpleClientGoalState MoveYoubotBaseClient::getState()
{
  if( !client->isServerConnected() ) return actionlib::SimpleClientGoalState::ABORTED;

  //!TODO: getting state while no goal is active leads to an error :( catch error!
  else return client->getState();
}

// +#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+ //

#if TESTING == 1

int main(int argc, char** argv) /////ONLY FOR TESTING PURPOSES//////
{
  MoveYoubotBaseClient *client = new MoveYoubotBaseClient(argc, argv);

  double x=0,y=0,theta=0;
  bool result;

  for(int i=0;i<10;i++)
  {
    if( i%2 ) result = client->move(2,1);
    else result = client->move(1,2);

    if( result ) printf("Yay! The youbot base moved.\n");
    else printf("The youbot seems to be sick :(\n");

    client->getCurrentPosition(&x,&y,&theta);
    printf("\tYoubots current position is: %d|%d, %d°\n",x,y,theta);
  }

  return 0;

}

#endif
